/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <wpb_cv_local_planner/wpb_cv_local_planner.h>
#include <tf_conversions/tf_eigen.h>
#include <pluginlib/class_list_macros.h>

// register this planner as a WpbCVLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS( wpb_cv_local_planner::WpbCVLocalPlanner, nav_core::BaseLocalPlanner)

namespace wpb_cv_local_planner
{
    WpbCVLocalPlanner::WpbCVLocalPlanner()
    {
        //ROS_WARN("[WPB_CV]WpbCVLocalPlanner() ");
        m_costmap_ros = NULL;
        m_tf_listener = NULL; 
        m_goal_reached = false;
        m_bInitialized = false;
        m_bAC = false;
        m_bFirstStep = true;
    }

    WpbCVLocalPlanner::~WpbCVLocalPlanner()
    {
    }

    void WpbCVLocalPlanner::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
    {
        ROS_WARN("WpbCVLocalPlanner::initialize() ");
        if(!m_bInitialized)
        {	
            m_tf_listener = tf;
            m_costmap_ros = costmap_ros;
            m_costmap = m_costmap_ros->getCostmap();
            
            m_global_frame_id = m_costmap_ros->getGlobalFrameID();      //"odom"
            m_robot_base_frame_id = m_costmap_ros->getBaseFrameID();    //"base_footprint"
            
            m_footprint_spec = m_costmap_ros->getRobotFootprint();
            costmap_2d::calculateMinAndMaxDistances(m_footprint_spec, m_robot_inscribed_radius, m_robot_circumscribed_radius); 

            ros::NodeHandle nh_planner("~/" + name);
            nh_planner.param("max_vel_trans", m_max_vel_trans, 0.3);
            nh_planner.param("max_vel_rot", m_max_vel_rot, 0.9);
            nh_planner.param("acc_scale_trans", m_acc_scale_trans, 0.8);
            nh_planner.param("acc_scale_rot", m_acc_scale_rot, 0.3);
            nh_planner.param("goal_dist_tolerance", m_goal_dist_tolerance, 0.2);
            nh_planner.param("goal_yaw_tolerance", m_goal_yaw_tolerance, 0.2);
            nh_planner.param("scan_topic", m_scan_topic, std::string("/scan"));

            m_pub_target = nh_planner.advertise<geometry_msgs::PoseStamped>("local_planner_target", 10);
            m_scan_sub = nh_planner.subscribe<sensor_msgs::LaserScan>(m_scan_topic,1,&WpbCVLocalPlanner::lidarCallback,this);
            
            m_bInitialized = true;

            ROS_DEBUG("wpb_cv_local_planner plugin initialized.");
        }
        else
        {
            ROS_WARN("wpb_cv_local_planner has already been initialized, doing nothing.");
        }
    }

    void WpbCVLocalPlanner::initialize(std::string name,tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
     {
        ROS_WARN("WpbCVLocalPlanner::initialize() ");
        if(!m_bInitialized)
        {	
            m_tf_listener = new tf::TransformListener;
            m_costmap_ros = costmap_ros;
            m_costmap = m_costmap_ros->getCostmap();
            
            m_global_frame_id = m_costmap_ros->getGlobalFrameID();      //"odom"
            m_robot_base_frame_id = m_costmap_ros->getBaseFrameID();    //"base_footprint"
            
            m_footprint_spec = m_costmap_ros->getRobotFootprint();
            costmap_2d::calculateMinAndMaxDistances(m_footprint_spec, m_robot_inscribed_radius, m_robot_circumscribed_radius); 

            ros::NodeHandle nh_planner("~/" + name);
            nh_planner.param("max_vel_trans", m_max_vel_trans, 0.3);
            nh_planner.param("max_vel_rot", m_max_vel_rot, 0.9);
            nh_planner.param("acc_scale_trans", m_acc_scale_trans, 0.8);
            nh_planner.param("acc_scale_rot", m_acc_scale_rot, 0.3);
            nh_planner.param("goal_dist_tolerance", m_goal_dist_tolerance, 0.2);
            nh_planner.param("goal_yaw_tolerance", m_goal_yaw_tolerance, 0.2);
            nh_planner.param("scan_topic", m_scan_topic, std::string("/scan"));

            m_pub_target = nh_planner.advertise<geometry_msgs::PoseStamped>("local_planner_target", 10);
            m_scan_sub = nh_planner.subscribe<sensor_msgs::LaserScan>(m_scan_topic,1,&WpbCVLocalPlanner::lidarCallback,this);
            
            m_bInitialized = true;

            ROS_DEBUG("wpb_cv_local_planner plugin initialized.");
        }
        else
        {
            ROS_WARN("wpb_cv_local_planner has already been initialized, doing nothing.");
        }
    }

    static double fScaleD2R = 3.14159 / 180;
    void WpbCVLocalPlanner::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
    {
        //ROS_INFO("WpbCVLocalPlanner::lidarCallback");
        float left_obst = 1.0;
        float right_obst = 1.0;

        int nRanges = scan->ranges.size();
        float range_max = scan->range_max;
        float range_min = scan->range_min;
        float ac_width = 0.20;
        float ac_dist = 0.45;

        //right front
        for(int i=90;i<180;i++)
        {
             if(scan->ranges[i] < range_min || scan->ranges[i] > range_max)
                continue;
            float dist = scan->ranges[i] * sin((i-90)*fScaleD2R);
            if(dist < ac_dist)
            {
                float width = scan->ranges[i] * cos((i-90)*fScaleD2R);
                if(width < right_obst)
                {
                    right_obst = width;
                }
            }
        }

        //left front
        for(int i=180;i<270;i++)
        {
             if(scan->ranges[i] < range_min || scan->ranges[i] > range_max)
                continue;
            float dist = scan->ranges[i] * sin((270-i)*fScaleD2R);
            if(dist < ac_dist)
            {
                float width = scan->ranges[i] * cos((270-i)*fScaleD2R);
                if(width < left_obst)
                {
                    left_obst = width;
                }
            }
        }

        // ajust
        if(right_obst > ac_width && left_obst > ac_width)
        {
            m_bAC = false;
        }
        else
        {
            m_bAC = true;
            if(right_obst < left_obst)
            {
                //m_nAC_action = AC_TURN_LEFT;
            }
            else
            {
                //m_nAC_action = AC_TURN_RIGHT;
            }
        }
    }

    bool WpbCVLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& plan)
    {
        // ROS_WARN("[WPB_CV]setPlan() ");
        if(!m_bInitialized)
        {
            ROS_ERROR("wpb_cv_local_planner has not been initialized, please call initialize() before using this planner");
            return false;
        }

        m_global_plan.clear();
        m_global_plan = plan;
        m_nPathIndex = 0;

        m_goal_reached = false;
        m_nStep = WPB_CV_STEP_GOTO;
        m_bFirstStep = true;
        
        return true;
    }

    static double CalDirectAngle(double inFromX, double inFromY, double inToX, double inToY)
    {
        double res = 0;
        double dx = inFromX - inToX;
        double dy = -(inFromY - inToY);
        if (dx == 0)
        {
            if (dy > 0)
            {
                res = 180 - 90;
            }
            else
            {
                res = 0 - 90;
            }
        }
        else
        {
            double fTan = dy / dx;
            res = atan(fTan) * 180 / 3.1415926;

            if (dx < 0)
            {
                res = res - 180;
            }
        }
        res = 180 - res;
        if (res < 0)
        {
            res += 360;
        }
        if (res > 360)
        {
            res -= 360;
        }
        res = res*3.1415926/180;
        return res;
    }

    static double AngleFix(double inAngle, double inMin, double inMax)
    {
        if (inMax - inMin > 6.28)
        {
            return inAngle;
        }
        
        double retAngle = inAngle;
        while (retAngle < inMin)
        {
            retAngle += 6.28;
        }
        while (retAngle > inMax)
        {
            retAngle -= 6.28;
        }
        return retAngle;
    }

    bool WpbCVLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
    {
        // ROS_WARN("[WPB_CV]computeVelocityCommands() ");
        if(!m_bInitialized)
        {
            ROS_ERROR("wpb_cv_local_planner has not been initialized, please call initialize() before using this planner");
            return false;
        }

        int path_num = m_global_plan.size();
        if(path_num == 0)
        {
            return false;
        }

        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.angular.z = 0;

        /////////////////////////////////////////////////
        if(m_bFirstStep == true)
        {
            double target_x, target_y, target_th;
            while(m_nPathIndex < path_num-1)
            {
                getTransformedPosition(m_global_plan[m_nPathIndex], m_robot_base_frame_id, target_x, target_y, target_th);
                if(sqrt(target_x*target_x + target_y*target_y) < m_goal_dist_tolerance)
                {
                    m_nPathIndex ++;
                    //ROS_WARN("[WPB_CV-GOTO]target = %d ",m_nPathIndex);
                }
                else
                {
                    break;  //target is far enough
                }
            }

            double face_target = CalDirectAngle(0, 0, target_x, target_y);
            face_target = AngleFix(face_target,-2.1,2.1);
            if(fabs(face_target)> 0.09)
            {
                //turn in place
                cmd_vel.linear.x = 0;
                cmd_vel.linear.y = 0;
                cmd_vel.angular.z = face_target * m_acc_scale_rot;
                if(cmd_vel.angular.z > 0) cmd_vel.angular.z +=0.2;
                if(cmd_vel.angular.z < 0) cmd_vel.angular.z -=0.2;
            }
            else
            {
                m_bFirstStep = false;
            }
        }
        ////////////////////////////////////////////////////////
        
        if(m_nStep == WPB_CV_STEP_ARRIVED)
        {
            ROS_WARN("[WPB_CV_ARRIVED](%.2f %.2f):%.2f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
            return true;
        }

         /////////////////////////////////////////////////////////////
        //getTransformedPosition(m_global_plan[m_nPathIndex], m_robot_base_frame_id, gx, gy, gth);
        double goal_x,goal_y,goal_th;
        getTransformedPosition(m_global_plan.back(), m_robot_base_frame_id, goal_x, goal_y, goal_th);
        //ROS_WARN("goal(%.2f dy= %.2f) th= %.2f",goal_x, goal_y, goal_th);

        //  double face_goal = CalDirectAngle(0, 0, goal_x, goal_y);
        //  face_goal = AngleFix(face_goal,-2.1,2.1);
        //  ROS_WARN("face = %.2f goal(%.2f dy= %.2f) th= %.2f",face_goal, goal_x, goal_y, goal_th);
        
        if(m_nStep == WPB_CV_STEP_GOTO)
        {
            // check if global goal is near
            double goal_dist = sqrt(goal_x*goal_x + goal_y*goal_y);
            if(goal_dist < m_goal_dist_tolerance)
            {
                m_nStep = WPB_CV_STEP_NEAR;
                //ROS_WARN("[WPB_CV-GOTO] -> [WPB_CV_NEAR] (%.2f,%.2f) %.2f",goal_x, goal_y, goal_th);
            }
            else
            {
                //check if target is near
                double target_x, target_y, target_th;
                while(m_nPathIndex < path_num-1)
                {
                    getTransformedPosition(m_global_plan[m_nPathIndex], m_robot_base_frame_id, target_x, target_y, target_th);
                    if(sqrt(target_x*target_x + target_y*target_y) < m_goal_dist_tolerance)
                    {
                        m_nPathIndex ++;
                        //ROS_WARN("[WPB_CV-GOTO]target = %d ",m_nPathIndex);
                    }
                    else
                    {
                        break;  //target is far enough
                    }
                }

                double face_target = CalDirectAngle(0, 0, target_x, target_y);
                face_target = AngleFix(face_target,-2.1,2.1);
                if(fabs(face_target)> 0.32)
                {
                    //turn in place
                    cmd_vel.linear.x = 0;
                    cmd_vel.linear.y = 0;
                    cmd_vel.angular.z = face_target * m_acc_scale_rot;
                    if(cmd_vel.angular.z > 0) cmd_vel.angular.z +=0.2;
                    if(cmd_vel.angular.z < 0) cmd_vel.angular.z -=0.2;
                }
                else
                {
                    double target_dist = sqrt(target_x*target_x + target_y*target_y);
                    cmd_vel.linear.x = target_dist*cos(face_target) * m_acc_scale_trans;
                    cmd_vel.linear.y = target_dist*sin(face_target) * m_acc_scale_trans;
                    cmd_vel.angular.z = face_target * m_acc_scale_rot;
                    if(cmd_vel.linear.x > 0) cmd_vel.linear.x+=0.05;
                    if(cmd_vel.linear.x < 0) cmd_vel.linear.x-=0.05;
                    if(cmd_vel.linear.y > 0) cmd_vel.linear.y+=0.02;
                    if(cmd_vel.linear.y < 0) cmd_vel.linear.y-=0.02;

                    // cmd_vel.linear.x = 0;
                    // cmd_vel.linear.y = 0;
                    if(m_bAC == true)
                    {
                        m_bAC = false;
                        cmd_vel.linear.x = 0;
                        cmd_vel.linear.y = 0;
                        cmd_vel.angular.z = 0;
                        return m_bAC;
                    }
                }
                m_pub_target.publish(m_global_plan[m_nPathIndex]);
            }
        }

        if(m_nStep == WPB_CV_STEP_NEAR)
        {
            
            cmd_vel.linear.x = 0;
            cmd_vel.linear.y = 0;
            cmd_vel.angular.z = goal_th;

            if(fabs(goal_th) < m_goal_yaw_tolerance)
            {
                m_goal_reached = true;
                m_nStep = WPB_CV_STEP_ARRIVED;
                cmd_vel.angular.z = 0;
                ROS_WARN("[WPB_CV-ARRIVED] goal (%.2f,%.2f) %.2f",goal_x, goal_y, goal_th);
            }
            m_pub_target.publish(m_global_plan.back());
        }

        if(cmd_vel.linear.x > m_max_vel_trans) cmd_vel.linear.x = m_max_vel_trans;
        if(cmd_vel.linear.x < -m_max_vel_trans) cmd_vel.linear.x = -m_max_vel_trans;
        if(cmd_vel.linear.y > m_max_vel_trans) cmd_vel.linear.y = m_max_vel_trans;
        if(cmd_vel.linear.y < -m_max_vel_trans) cmd_vel.linear.y = -m_max_vel_trans;
        if(cmd_vel.angular.z > m_max_vel_rot) cmd_vel.angular.z = m_max_vel_rot;
        if(cmd_vel.angular.z < -m_max_vel_rot) cmd_vel.angular.z = -m_max_vel_rot;

        m_last_cmd = cmd_vel;
        
        return true;
    }


    bool WpbCVLocalPlanner::isGoalReached()
    {
        //ROS_WARN("[WPB_CV]isGoalReached() ");
        if (m_goal_reached)
        {
            ROS_INFO("GOAL Reached!");
            return true;
        }
        return false;
    }

    void WpbCVLocalPlanner::getTransformedPosition(geometry_msgs::PoseStamped& pose, std::string& frame_id, double& x, double& y, double& theta)
    {
        geometry_msgs::PoseStamped tf_pose;
        pose.header.stamp = ros::Time(0);
        m_tf_listener->transformPose(frame_id, pose, tf_pose);
        x = tf_pose.pose.position.x;
        y = tf_pose.pose.position.y,
        theta = tf::getYaw(tf_pose.pose.orientation);
    }

}